#include "MainScene.h"
#include "math.h"
// Connect callback
static bool g_SocketIsConnected;
static int32 ConnectCB(s3eSocket* g_Sock, void* sys, void* data)
{
    s3eResult res = *(s3eResult*)sys;

    if (res == S3E_RESULT_SUCCESS)
        g_SocketIsConnected = true;
    else
        g_SocketIsConnected = false;

    return 0;
}

bool MainScene::init()
{
	if (!CCLayer::init())
	{
		return false;
	}

	CCSize winSize = CCDirector::sharedDirector()->getWinSize();
	
	CCMenuItemImage* calibrateButton = CCMenuItemImage::itemFromNormalImage("images/red.png","images/red.png","images/red.png",this, menu_selector(MainScene::reset));
	calibrateButton->setPosition(ccp(winSize.width*0.5f,winSize.height*0.5f));
	calibrateButton->setScale(0.25f);

	yellowMask = CCSprite::spriteWithFile("images/yellow.png");
	yellowMask->setPosition(ccp(winSize.width*0.5f,winSize.height*0.5f));
	yellowMask->setOpacity(0);
	yellowMask->setScale(0.25f);

	greenMask = CCSprite::spriteWithFile("images/green.png");
	greenMask->setPosition(ccp(winSize.width*0.5f,winSize.height*0.5f));
	greenMask->setOpacity(0);
	greenMask->setScale(0.25f);

	CCMenu* menu = CCMenu::menuWithItems(calibrateButton,NULL);
	menu->setPosition(ccp(0,0));

	addChild(menu);
	addChild(yellowMask);
	addChild(greenMask);

	CCLabelBMFont* title = CCLabelBMFont::labelWithString("PelePong Client", "fonts/foo18.fnt");
	title->setPosition(ccp(winSize.width*0.5f,winSize.height*0.9f));
	addChild(title);	

	status = CCLabelBMFont::labelWithString("idle", "fonts/foo18.fnt");
	status->setPosition(ccp(winSize.width*0.5f,winSize.height*0.1f));
	addChild(status);

	// openning connection to server
	g_Sock = s3eSocketCreate(S3E_SOCKET_UDP, 0);
	if (g_Sock == NULL)
    {
        status->setString("Creation of socket failed");
        return true;
    }
	status->setString("Creation of socket success");

	s3eInetAddress addr;
    memset(&addr, 0, sizeof(addr));
	char hostname[128];
	int port;
	s3eConfigGetString("Server","hostname",hostname);
	s3eConfigGetInt("Server","port",&port);
    if (s3eInetLookup(hostname, &addr, NULL, NULL) == S3E_RESULT_ERROR)
    {
        status->setString("s3eInetLookup failed");
        return true;
    }
	status->setString("s3eInetLookup success");

    addr.m_Port = s3eInetHtons(port);

	if (s3eSocketConnect(g_Sock, &addr, &ConnectCB, NULL) != S3E_RESULT_SUCCESS)
    {
		switch (s3eSocketGetError())
        {
            // These errors are 'OK', because they mean,
            // that a connect is in progress
            case S3E_SOCKET_ERR_INPROGRESS:
            case S3E_SOCKET_ERR_ALREADY:
            case S3E_SOCKET_ERR_WOULDBLOCK:
                break;
            default:
                // A 'real' error happened
                status->setString("Connecting failed");
                s3eSocketClose(g_Sock);
                return true;
        }
	}

	status->setString("leave the device on the center position\nand touch the screen to calibrate");
	schedule(schedule_selector(MainScene::mainLoop),0.0001f);
	//schedule(schedule_selector(MainScene::debugLoop),0.0001f);
	calibrationState = IDLE;
	started = false;

	// register pause event
	s3eDeviceRegister(S3E_DEVICE_PAUSE, &pauseCallback, NULL);
	s3eDeviceRegister(S3E_DEVICE_BACKGROUND, &pauseCallback, NULL);
	s3eDeviceRegister(S3E_DEVICE_EXIT, &pauseCallback, NULL);
	return true;
}

int32 MainScene::pauseCallback(void* systemData, void* userData)
{
	CCDirector::sharedDirector()->end();
	s3eDeviceExit();
	return 0;
}

void MainScene::reset(CCObject* sender)
{
	if (calibrationState == READY || calibrationState == IDLE)
	{
		calibrationState = UNCALIBRATED;
		started = true;
		x = 0;
		y = 0;
		prevax = 0;
		prevay = 0;	

		rpreva=0;
		rprevv=0;
		rv=0;
		rx=0;
	}
	else if (calibrationState == CALIBRATED)
	{
		calibrationState = READY;
		greenMask->setOpacity(255);
		yellowMask->setOpacity(0);
		status->setString("ready");
		x = (rightx + leftx) /2;
	}
	else
	{
		// do nothing, in a middle of calibration process
	}
}

void MainScene::debugLoop(ccTime time)
{
	int32 ax = s3eAccelerometerGetX();
	int32 ay = s3eAccelerometerGetY();
	int32 az = s3eAccelerometerGetZ();
	uint64 t = s3eTimerGetMs();

	char messageToSend[128];
	sprintf(messageToSend, "%d,%d,%d,%d", ax,ay,az,(int)t);
	int32       messageLength = strlen(messageToSend);
	int32       messageSent = 0;
	int32       retValue;

	// returns the number of bytes sent, or -1 on failure.
	retValue = s3eSocketSend(g_Sock, messageToSend+messageSent, messageLength-messageSent, 0);
}

void MainScene::mainLoop(ccTime time)
{	
	if (started)
	{
		// prevent device from sleeping
		s3eDeviceBacklightOn();

		int32 ax = s3eAccelerometerGetX();
		int32 ay = s3eAccelerometerGetY();
		int32 az = s3eAccelerometerGetZ();
		uint64 t = s3eTimerGetMs();
		static uint64 lastt;

		switch (calibrationState)
		{
		case UNCALIBRATED:
			maxsax = 0;
			maxsay = 0;
			status->setString("calibrating... please wait");
			greenMask->setOpacity(0);
			yellowMask->setOpacity(255);
			calibrationState = CALIBRATE_CENTER;
			break;
		case CALIBRATE_CENTER:
			calibrate(ax, ay, az, t);
			break;
		case CALIBRATE_LEFT:
			position(ax, ay, az, t);
			if (movementState == STOPPED)
			{
				leftx = x;
				calibrationState = CALIBRATE_RIGHT;
				status->setString("move the device as fast as possible\nto the right border of the table");
				lastt = t;
			}
			break;
		case CALIBRATE_RIGHT:
			if (t - lastt > 500)
			{
				position(ax, ay, az, t);
				if (movementState == STOPPED)
				{
					rightx = x;
					calibrationState = CALIBRATED;
					greenMask->setOpacity(0);
					yellowMask->setOpacity(255);
					status->setString("device calibrated, move device to the center\nagain and touch the screen when ready");
				}
			}
			break;
		case CALIBRATED:
			break;
		case READY:
			position(ax, ay, az, t);
		//	if (movementState == STOPPED)
		//	{
				handle_position();

				char messageToSend[20];
				sprintf(messageToSend, "1~%1.0f:%1.0f", -y, -1000+(x -rightx)/(leftx-rightx)*2000);
				int32       messageLength = strlen(messageToSend);
				int32       messageSent = 0;
				int32       retValue;

				// returns the number of bytes sent, or -1 on failure.
				retValue = s3eSocketSend(g_Sock, messageToSend+messageSent, messageLength-messageSent, 0);
		/*		rx=x;
				rprevv=0;
			} else {
				if (ax != prevax)
				{
					double dt = (t - prevt) / 1000.0f;
		
					// convert measures to m/s^2 and decrease static value
					double a = ax * 9.81f / 1000.0f - staticax;
				
					rv = rv + ((a + rpreva) / 2) * dt;
					rx = rx + ((rv + rprevv) / 2) * dt;
					rprevv=rv;
					rpreva=a;
				
					char messageToSend[20];
					sprintf(messageToSend, "1~%1.0f:%1.0f", -y, -1000+(rx -rightx)/(leftx-rightx)*2000);
					int32       messageLength = strlen(messageToSend);
					int32       messageSent = 0;
					int32       retValue;

					// returns the number of bytes sent, or -1 on failure.
					retValue = s3eSocketSend(g_Sock, messageToSend+messageSent, messageLength-messageSent, 0);
				}

			}*/
			break;
		}		
	}
}

void MainScene::calibrate(double ax, double ay, double az, int64 t)
{
	static int count = 0;
	static double sumax = 0, sumay = 0;	
	if (count < 64)
	{
		sumax += ax;
		sumay += ay;
		if (ABS(ax) > maxsax) maxsax = ABS(ax);
		if (ABS(ay) > maxsay) maxsay = ABS(ay);
		count++;
	}
	else
	{
		// calculate static values for ax,ay
		staticax = sumax / count;
		staticay = sumay / count;
		maxsax-=staticax;
		maxsay-=staticay;

		// convert values to m/s^2
		staticax = staticax * 9.81f / 1000.0f;
		staticay = staticay * 9.81f / 1000.0f;
		maxsax = maxsax * 9.81f / 1000.0f;
		maxsay = maxsay * 9.81f / 1000.0f;

		calibrationState = CALIBRATE_LEFT;
		movementState = MOVEMENT_IDLE;
		prevt = t;
		prevax = ax;
		prevay = ay;
		count = 0;
		yellowMask->setOpacity(0);
		greenMask->setOpacity(0);
		status->setString("move the device as fast as possible\nto the left border of the table");
	}	
}

void MainScene::position(double ax, double ay, double az, int64 t)
{
	static int sampleCount;
	static int zeroCount;
	static int direction;
	static double samples[MAX_SAMPLES];
	static double sample_times[MAX_SAMPLES];
	static double suma;

	if (ax != prevax)
	{
		// calc delta time
		double dt = (t - prevt) / 1000.0f;
		
		// convert measures to m/s^2 and decrease static value
		double a = ax * 9.81f / 1000.0f - staticax;
		bool isZero = (ABS(a) < maxsax);
		if (!isZero) zeroCount = 0;

		switch (movementState)
		{
		case MOVEMENT_IDLE:
			if (!isZero)
			{
				sampleCount = 2;
				zeroCount = 0;
				samples[1] = a;
				sample_times[1] = (t - realprevt) / 1000.0f;
				suma = a;				
				movementState = POSITIVE;
			}
			else
			{
				// store the last zero (it will be the first value of the sample list)
				samples[0] = a;
				sample_times[0] = 0;
			}
			break;
		case POSITIVE:
			{		
				if (sampleCount == 2)
				{
					direction = (a > samples[1])?1:-1;
				}

				samples[sampleCount] = a;
				sample_times[sampleCount] = dt;
				sampleCount++;
				suma+=a;

				if (direction * a < -maxsax) movementState = NEGATIVE; 
				
				// handle zero
				if (isZero) zeroCount++;
				if (zeroCount > MAX_ZEROS)
				{
					// bad movement, switch back to idle
					movementState = MOVEMENT_IDLE;
				}
			}
			break;
		case NEGATIVE:
			{				
				samples[sampleCount] = a;
				sample_times[sampleCount] = dt;
				sampleCount++;
				suma+=a;
								
				// handle zero
				if (isZero) zeroCount++;
				if (zeroCount > MAX_ZEROS || direction * a > maxsax )
				{
					// too many zero values, i guess the movment stopped
					movementState = STOPPED;

					// remove zero samples in the end
					sampleCount-=(zeroCount-1);
				}
				if (movementState == STOPPED)
				{
					// fixing the samples & calculate distance 
					double fixa = -1 * (suma / sampleCount);
					double v,prevv,preva;
					preva = 0;
					prevv = 0;
					v = 0;
					for (int i=0; i<sampleCount; i++)
					{
						double aFixed = samples[i] + fixa;
						double dt = sample_times[i];

						v = v + ((aFixed + preva) / 2) * dt;
						x = x + ((v + prevv) / 2) * dt;

						prevv = v;
						preva = aFixed;
					}
				}
			}
			break;
		case STOPPED:
			// back to idle state
			movementState = MOVEMENT_IDLE;
			break;
		}

		// save prev values
		prevt = t;
		prevax = ax;
	}
	realprevt = t;
}

void MainScene::handle_position()
{
	if (x < rightx) 
	{
		double delta = rightx - x;
		leftx-=delta;
		rightx-=delta;
	}
	if (x > leftx) 
	{
		double delta = x - leftx;
		leftx+=delta;
		rightx+=delta;
	}

	char messageToSend[128];
	sprintf(messageToSend, "left:%1.1f, right:%1.1f, x:%1.1f,y:%1.1f", leftx, rightx, x, y);
	int32       messageLength = strlen(messageToSend);
	int32       messageSent = 0;

	status->setString(messageToSend);	
}
/*
void MainScene::rhandle_position()
{
	if (rx < rightx) 
	{
		double delta = rightx - rx;
		//leftx-=delta;
		//rightx-=delta;
	}
	if (rx > leftx) 
	{
		double delta = rx - leftx;
		//leftx+=delta;
		//rightx+=delta;
	}

	char messageToSend[128];
	sprintf(messageToSend, "left:%1.1f, right:%1.1f, x:%1.1f,y:%1.1f", leftx, rightx, rx, ry);
	int32       messageLength = strlen(messageToSend);
	int32       messageSent = 0;

	status->setString(messageToSend);	
}*/